#include "RigidBody.h"
#include "RigidBodyMotionState.h"
#include "System.h"

RigidBody::RigidBody( float mass, void* userPointer, Maths::Vector3D& position, Maths::Vector3D& rotation ) : m_system(0)
{
	m_mass = mass;
	m_position = position;
	m_rotation = rotation;
	if (userPointer == 0)
		userPointer = this;
	Initialize( mass, position, rotation, userPointer );
}

RigidBody::~RigidBody()
{
	Deinitialize();
}

void RigidBody::Initialize( float mass, Maths::Vector3D& position, Maths::Vector3D& rotation, void* userPointer )
{
	m_shape = new btCompoundShape();
	m_motionState = new RigidBodyMotionState(this, btTransform(btQuaternion(rotation.X(), rotation.Y(), rotation.Z()), btVector3(position.X(), position.Y(), position.Z())));
	btRigidBody::btRigidBodyConstructionInfo body(mass, m_motionState, m_shape, btVector3(0, 0, 0));
	m_rigidBody = new btRigidBody(body);
	m_rigidBody->setUserPointer(userPointer);

	//if (mass > 0)
		//m_rigidBody->setCollisionFlags(btRigidBody::CollisionFlags::CF_STATIC_OBJECT);
	 //m_rigidBody->setCollisionFlags(btRigidBody::CF_KINEMATIC_OBJECT);
}

void RigidBody::Deinitialize()
{
	if (m_system)
		m_system->RemoveRigidBody(this);
}

void RigidBody::setSystem( System* system )
{
	m_system = system;
}

System* RigidBody::getSystem( )
{
	return m_system;
}

Subject<Maths::Vector3D>& RigidBody::getPosition()
{
	return m_position;
}

Subject<Maths::Quaternion>& RigidBody::getRotation()
{
	return m_rotation;
}

void RigidBody::AddShape( Pointer<IShape>& shape )
{
	btTransform principal(btQuaternion(0,0,0,1));
	btVector3 localInertia(0,0,0);
	btVector3 principalInertia(0,0,0);

	//m_shape = shape->getShape();

	// Add shape
	m_childShapes.push_back(shape);
	m_shape->addChildShape(shape->getLocalOffset(), shape->getShape());
	// Mass per shape
	if (m_mass > 0)
	{
		Pointer<float> masses = new float[m_shape->getNumChildShapes()];
		for (int i = 0; i < m_shape->getNumChildShapes(); ++i)
			masses[i] = m_mass / m_shape->getNumChildShapes();
	
		m_shape->calculatePrincipalAxisTransform(masses,principal,principalInertia);

		// update the transforms
		for (int i = 0; i < m_shape->getNumChildShapes(); ++i)
		{
			btTransform newChildTransform = principal.inverse() * m_shape->getChildTransform(i);
			m_shape->updateChildTransform(i,newChildTransform);
		}
	}

	// inertia
	if (m_mass > 0)
		m_shape->calculateLocalInertia(m_mass,localInertia);
	
	// rebuild body
	System* system = m_system;
	if (system)
		system->RemoveRigidBody(this);

	btRigidBody::btRigidBodyConstructionInfo rbInfo(m_mass,m_motionState,m_shape,localInertia);
	void* userPointer = m_rigidBody->getUserPointer();
	m_rigidBody = new btRigidBody(rbInfo);
	m_rigidBody->setUserPointer(userPointer);
	if (m_mass == 0)
		m_rigidBody->setCollisionFlags(btRigidBody::CollisionFlags::CF_STATIC_OBJECT);
	
	if (system)
		system->AddRigidBody(this);
}

Pointer<btRigidBody>& RigidBody::getRigidBody()
{
	return m_rigidBody;
}

void RigidBody::SetStatic()
{
	m_rigidBody->setLinearFactor(btVector3(0,0,0));
	m_rigidBody->setAngularFactor(btVector3(0,0,0));
}

void RigidBody::Accelerate(Maths::Vector3D& linearVelocity)
{
	btVector3 vel(linearVelocity.X(), linearVelocity.Y(), linearVelocity.Z());
	btVector3 curvel = m_rigidBody->getLinearVelocity();
	curvel += vel;
	m_rigidBody->setLinearVelocity(curvel);
}

void RigidBody::setPosition( const Maths::Vector3D& position )
{
	btVector3 localInertia(0,0,0);
	// rebuild body
	System* system = m_system;
	if (system)
		system->RemoveRigidBody(this);
	// inertia
	if (m_mass > 0)
		m_shape->calculateLocalInertia(m_mass,localInertia);

	btVector3 new_position(position.X(), position.Y(), position.Z());
	Maths::Quaternion& rotation = m_rotation.getObject();
	btTransform transform = m_rigidBody->getCenterOfMassTransform();
	transform.setOrigin(new_position);

	m_motionState = new RigidBodyMotionState(this, btTransform(btQuaternion(rotation.X(), rotation.Y(), rotation.Z(), rotation.W()), btVector3(position.X(), position.Y(), position.Z())));
	
	btRigidBody::btRigidBodyConstructionInfo rbInfo(m_mass,m_motionState,m_shape,localInertia);
	void* userPointer = m_rigidBody->getUserPointer();
	m_rigidBody = new btRigidBody(rbInfo);
	m_rigidBody->setUserPointer(userPointer);
	if (m_mass == 0)
		m_rigidBody->setCollisionFlags(btRigidBody::CollisionFlags::CF_STATIC_OBJECT);
	
	m_rigidBody->setCenterOfMassTransform(transform);

	if (system)
		system->AddRigidBody(this);
}

void RigidBody::setRotation( Maths::Quaternion& rotation )
{
	m_rotation = rotation;
	btVector3 localInertia(0,0,0);
	if (m_mass > 0)
		m_shape->calculateLocalInertia(m_mass,localInertia);
	
	System* system = m_system;
	if (system)
		system->RemoveRigidBody(this);

	m_motionState = new RigidBodyMotionState(this, btTransform(btQuaternion(rotation.X(), rotation.Y(), rotation.Z(), rotation.W()), btVector3(m_position.getObject().X(), m_position.getObject().Y(), m_position.getObject().Z())));
	btRigidBody::btRigidBodyConstructionInfo body(m_mass, m_motionState, m_shape, btVector3(0, 0, 0));
	m_rigidBody = new btRigidBody(body);
	if (m_mass == 0)
		m_rigidBody->setCollisionFlags(btRigidBody::CF_STATIC_OBJECT);

	if (system)
		system->AddRigidBody(this);
}

void RigidBody::ClearShapes()
{
	btTransform principal(btQuaternion(0,0,0,1));
	btVector3 localInertia(0,0,0);
	btVector3 principalInertia(0,0,0);
	// Add shape
	for (int i = 0; i < m_childShapes.size(); ++i)
		m_shape->removeChildShapeByIndex(0);
	m_childShapes.clear();
	
	// rebuild body
	System* system = m_system;
	if (system)
		system->RemoveRigidBody(this);

	btRigidBody::btRigidBodyConstructionInfo rbInfo(m_mass,m_motionState,m_shape,localInertia);
	void* userPointer = m_rigidBody->getUserPointer();
	m_rigidBody = new btRigidBody(rbInfo);
	m_rigidBody->setUserPointer(userPointer);
	//if (m_mass == 0)
		//m_rigidBody->setCollisionFlags(btRigidBody::CollisionFlags::CF_STATIC_OBJECT);
	
	if (system)
		system->AddRigidBody(this);
}

std::vector<Pointer<IShape>>& RigidBody::getShapes()
{
	return m_childShapes;
}

void RigidBody::setMass( float mass )
{	
	m_mass = mass;
	// rebuild body
	btVector3 localInertia(0,0,0);
	if (m_mass > 0)
		m_shape->calculateLocalInertia(m_mass,localInertia);
	
	System* system = m_system;
	if (system)
		system->RemoveRigidBody(this);

	btRigidBody::btRigidBodyConstructionInfo rbInfo(m_mass,m_motionState,m_shape,btVector3(0,0,0));
	void* userPointer = m_rigidBody->getUserPointer();
	m_rigidBody = new btRigidBody(rbInfo);
	m_rigidBody->setUserPointer(userPointer);
	
	if (m_mass == 0)
		m_rigidBody->setCollisionFlags(btRigidBody::CF_STATIC_OBJECT);

	if (system)
		system->AddRigidBody(this);
}
void RigidBody::RemoveShape( unsigned int index )
{
	if (m_childShapes.size() > index)
	{
		btCollisionShape* toRemove = m_childShapes[index]->getShape();
		m_shape->removeChildShape(toRemove);
		m_childShapes.erase(m_childShapes.begin() + index);

		System* system = m_system;
		if (system)
			system->RemoveRigidBody(this);

		btRigidBody::btRigidBodyConstructionInfo rbInfo(m_mass,m_motionState,m_shape,btVector3(0,0,0));
		void* userPointer = m_rigidBody->getUserPointer();
		m_rigidBody = new btRigidBody(rbInfo);
		m_rigidBody->setUserPointer(userPointer);
	
		if (system && m_childShapes.size() > 0)
			system->AddRigidBody(this);
	}
}